#include "MollerTriTriIntersect.h"

#include "../ETBarycentric/BCoords.cpp"

#include <cmath>

double EPSILON = 1e-8;

double LessThanEpsilon(double i_number)
	{
	if (i_number <= EPSILON && i_number >= -EPSILON)
		return 0.0;
	return abs(i_number);
	}

Point3D CrossProduct(const Point3D& i_a, const Point3D& i_b)
	{
	return Point3D
		{
		i_a.m_y * i_b.m_z - i_a.m_z * i_b.m_y,
		i_a.m_z * i_b.m_x - i_a.m_x * i_b.m_z,
		i_a.m_x * i_b.m_y - i_a.m_y * i_b.m_x
		};
	};

double DotProduct(const Point3D& i_a, const Point3D& i_b)
	{
	return i_a.m_x * i_b.m_x + i_a.m_y * i_b.m_y + i_a.m_z * i_b.m_z;
	}

Point3D Substitute(const Point3D& i_a, const Point3D& i_b)
	{
	return Point3D{ i_a.m_x - i_b.m_x, i_a.m_y - i_b.m_y, i_a.m_z - i_b.m_z };
	};

bool PointInsideTriangle(const Triangle3D& i_triangle, const Point3D& i_point)
	{
	Point p = Point{ i_point.m_x, i_point.m_y, i_point.m_z };
	Point a = Point{ i_triangle.m_a.m_x, i_triangle.m_a.m_y, i_triangle.m_a.m_z };
	Point b = Point{ i_triangle.m_b.m_x, i_triangle.m_b.m_y, i_triangle.m_b.m_z };
	Point c = Point{ i_triangle.m_c.m_x, i_triangle.m_c.m_y, i_triangle.m_c.m_z };
	Triangle t = Triangle{ a, b, c };
	return InsideTriangle(t, p);
	}

double Abs(const Point3D& i_point)
	{
	return sqrt(i_point.m_x * i_point.m_x + i_point.m_y * i_point.m_y + i_point.m_z * i_point.m_z);
	}

void SortPair(std::pair<double, double>& io_pair)
	{
	if (io_pair.first > io_pair.second)
		{
		double temp = io_pair.first;
		io_pair.first = io_pair.second;
		io_pair.second = temp;
		}
	}

bool ComputeIntervals(
	double& o_a, double& o_b, double& o_c, double& o_x0, double& o_x1,
	double i_p0, double i_p1, double i_p2, double i_d0, double i_d1, double i_d2)
	{
	if (i_d0 * i_d1 > 0)
		{
		o_a = i_p2;
		o_b = (i_p0 - i_p2) * i_d2;
		o_c = (i_p1 - i_p2) * i_d2;
		o_x0 = i_d2 - i_d0;
		o_x1 = i_d2 - i_d1;
		return false;
		}
	else if (i_d0 * i_d2 > 0)
		{
		o_a = i_p1;
		o_b = (i_p0 - i_p1) * i_d1;
		o_c = (i_p2 - i_p1) * i_d1;
		o_x0 = i_d1 - i_d0;
		o_x1 = i_d1 - i_d2;
		return false;
		}
	else if (i_d1 * i_d2 > 0 || i_d0 != 0)
		{
		o_a = i_p0;
		o_b = (i_p1 - i_p0) * i_d0;
		o_c = (i_p2 - i_p0) * i_d0;
		o_x0 = i_d0 - i_d1;
		o_x1 = i_d0 - i_d2;
		return false;
		}
	else if (i_d1 != 0)
		{
		o_a = i_p1;
		o_b = (i_p0 - i_p1) * i_d1;
		o_c = (i_p2 - i_p1) * i_d1;
		o_x0 = i_d1 - i_d0;
		o_x1 = i_d1 - i_d2;
		return false;
		}
	else if (i_d2 != 0)
		{
		o_a = i_p2;
		o_b = (i_p0 - i_p2) * i_d2;
		o_c = (i_p1 - i_p2) * i_d2;
		o_x0 = i_d2 - i_d0;
		o_x1 = i_d2 - i_d1;
		return false;
		}
	return true;
	}

bool EdgeAgainstEdge(const Point3D& i_point, const Point3D& i_edge_start, const Point3D& i_edge_finish,
	size_t i_i0, size_t i_i1, double i_a_x, double i_a_y)
	{
	auto b_x = i_edge_start[i_i0] - i_edge_finish[i_i0];
	auto b_y = i_edge_start[i_i1] - i_edge_finish[i_i1];
	auto c_x = i_point[i_i0] - i_edge_start[i_i0];
	auto c_y = i_point[i_i1] - i_edge_start[i_i1];

	auto f = i_a_y * b_x - i_a_x * b_y;
	auto d = b_y * c_x - b_x * c_y;

	if ((f > 0 && d >= 0 && d <= f) || (f < 0 && d <= 0 && d >= f))
		{
		auto e = i_a_x * c_y - i_a_y * c_x;
		if (f > 0)
			{
			if (e >= 0 && e <= f)
				return true;
			}
		else
			{
			if (e <= 0 || e >= f)
				return true;
			}
		}
	return false;
	}

bool EdgeAgainstTriangle(const Point3D& i_first, const Point3D& i_second, const Triangle3D& i_triangle,
	size_t i_i0, size_t i_i1)
	{
	auto a_x = i_second[i_i0] - i_first[i_i0];
	auto a_y = i_second[i_i1] - i_first[i_i1];
	return (
					EdgeAgainstEdge(i_first, i_triangle.m_a, i_triangle.m_b, i_i0, i_i1, a_x, a_y) ||
					EdgeAgainstEdge(i_first, i_triangle.m_b, i_triangle.m_c, i_i0, i_i1, a_x, a_y) ||
					EdgeAgainstEdge(i_first, i_triangle.m_c, i_triangle.m_a, i_i0, i_i1, a_x, a_y));
	}

bool HandleCoplanarTriangles(const Point3D& i_n, const Triangle3D& i_first, const Triangle3D& i_second)
	{
	auto a = Point3D{LessThanEpsilon(i_n.m_x), LessThanEpsilon(i_n.m_y), LessThanEpsilon(i_n.m_z)};
	size_t i0, i1;

	if (a.m_x > a.m_y)
		{
		if (a.m_x > a.m_z)
			{
			i0 = 1;
			i1 = 2;
			}
		else
			{
			i0 = 0;
			i1 = 1;
			}
		}
	else
		{
		if (a.m_y > a.m_z)
			{
			i0 = 0;
			i1 = 2;
			}
		else
			{
			i0 = 0;
			i1 = 1;
			}
		}
	EdgeAgainstTriangle(i_first.m_a, i_first.m_b, i_second, i0, i1);
	EdgeAgainstTriangle(i_first.m_b, i_first.m_c, i_second, i0, i1);
	EdgeAgainstTriangle(i_first.m_c, i_first.m_a, i_second, i0, i1);

	if (PointInsideTriangle(i_first, i_second.m_a) || PointInsideTriangle(i_second, i_first.m_a))
		return true;
	return false;
	}

bool AreTrianglesIntersecting(const Triangle3D& i_first, const Triangle3D& i_second)
	{
	// Plane equation for 1st triangle
	auto N1 = CrossProduct(Substitute(i_first.m_b, i_first.m_a), Substitute(i_first.m_c, i_first.m_a));
	auto d1 = -DotProduct(N1, i_first.m_a);

	auto d2_a = DotProduct(N1, i_second.m_a) + d1;
	auto d2_b = DotProduct(N1, i_second.m_b) + d1;
	auto d2_c = DotProduct(N1, i_second.m_c) + d1;

	if (d2_a * d2_b > 0 && d2_a * d2_c > 0)
		return false;

	// Plane equation for 2nd triangle
	auto N2 = CrossProduct(Substitute(i_second.m_b, i_second.m_a), Substitute(i_second.m_c, i_second.m_a));
	auto d2 = -DotProduct(N2, i_second.m_a);

	auto d1_a = DotProduct(N2, i_first.m_a) + d2;
	auto d1_b = DotProduct(N2, i_first.m_b) + d2;
	auto d1_c = DotProduct(N2, i_first.m_c) + d2;

	if (d1_a * d1_b > 0 && d1_a * d1_c > 0)
		return false;

	auto i10n_direction = CrossProduct(N1, N2);
	double max = LessThanEpsilon(i10n_direction.m_x);
	double abs_m_y = LessThanEpsilon(i10n_direction.m_y);
	double abs_m_z = LessThanEpsilon(i10n_direction.m_z);
	size_t index = 0;
	if (abs_m_y > max)
		{
		max = abs_m_y;
		index = 1;
		}
	if (abs_m_z > max)
		{
		max = abs_m_z;
		index = 2;
		}

	auto point_1_1 = i_first.m_a[index];
	auto point_1_2 = i_first.m_b[index];
	auto point_1_3 = i_first.m_c[index];

	auto point_2_1 = i_second.m_a[index];
	auto point_2_2 = i_second.m_b[index];
	auto point_2_3 = i_second.m_c[index];

	bool coplanar = false;

	/* compute interval for triangle 1 */
	double a, b, c, x0, x1;
	coplanar = ComputeIntervals(a, b, c, x0, x1, point_1_1, point_1_2, point_1_3, d1_a, d1_b, d1_c);
	if (coplanar)
		return HandleCoplanarTriangles(N1, i_first, i_second);

	double d, e, f, y0, y1;
	coplanar = ComputeIntervals(d, e, f, y0, y1, point_2_1, point_2_2, point_2_3, d2_a, d2_b, d2_c);

	double tmp;
	std::pair<double, double> isect1, isect2;

	tmp = a * x0 * x1 * y0 * y1;
	isect1.first = tmp + b * x1 * y0 * y1;
	isect1.second = tmp + c * x0 * y0 * y1;

	tmp = d * x0 * x1 * y0 * y1;
	isect2.first = tmp + e * x0 * x1 * y1;
	isect2.second = tmp + f * x0 * x1 * y0;

	SortPair(isect1);
	SortPair(isect2);

	if (isect1.second < isect2.first || isect2.second < isect1.first)
		return false;

	return true;
	}